#include "rc.h"

uint8_t RC_CheckFrame(uint8_t* frame_buf)
{
  if(frame_buf[0] != RC_HEADER) return 0;
  return CRC16Check(frame_buf, RC_FRAME_SIZE, RC_INIT_CRC16);
}

void RC_Decode(RC* rc, uint8_t* frame_buf)
{
  rc->ch0 = (frame_buf[1] | (frame_buf[2] << 8)) & 0x07ff;          //!< Channel 0  
  rc->ch1 = ((frame_buf[2] >> 3) | (frame_buf[3] << 5)) & 0x07ff;   //!< Channel 1         
  rc->ch2 = ((frame_buf[3] >> 6) | (frame_buf[4] << 2) |                                 
  (frame_buf[5] << 10)) & 0x07ff;                                 //!< Channel 2    
  rc->ch3 = ((frame_buf[5] >> 1) | (frame_buf[6] << 7)) & 0x07ff;   //!< Channel 3   
  rc->s1 = ((frame_buf[6] >> 4) & 0x000c) >> 2;                    //!< Switch left         
  rc->s2 = ((frame_buf[6] >> 4) & 0x0003);                         //!< Switch right
}

uint8_t RC_Pop(RC* rc, Que* que)
{
  if(que->size != RC_FRAME_SIZE) return 0;
  if(!RC_CheckFrame(que->buf)) return 0;
  RC_Decode(rc, que->buf);
  QueClear(que);
}

